/*
 *  chaseTrajectories.cpp
 *  Ball and Plate
 *
 *  Created by Kenn Sebesta on 2008-10-03.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "chaseTrajectories.h"
#include "displayOpenCV.h"

void circleChase( SetPoint *setpoint, double runTime ) {
    double setPoint_x, setPoint_y, setPoint_vx, setPoint_vy;
    double tau=runTime*setpoint->ballSpeed/setpoint->pathRadius;
//    cvZero( setpoint->integrationErr );

    setPoint_x=setpoint->pathRadius*cos(tau)+cvmGet( setpoint->tableCenter, 0, 0 );
    setPoint_y=setpoint->pathRadius*sin(tau)+cvmGet( setpoint->tableCenter, 0, 1 );
    setPoint_vx=-setpoint->ballSpeed*sin(tau);
    setPoint_vy=setpoint->ballSpeed*cos(tau);
    
    cvmSet( setpoint->setPoint, 0, 0, setPoint_x );
    cvmSet( setpoint->setPoint, 1, 0, setPoint_vx );
    cvmSet( setpoint->setPoint, 2, 0, setPoint_y );
    cvmSet( setpoint->setPoint, 3, 0, setPoint_vy );
    
//    printf("pathRadius = %f, setPoint_x radius = %f \n", setpoint->pathRadius, setpoint->pathRadius*cos(tau));
    
    return;
}

void ellipseChase( SetPoint *setpoint, double runTime ) {
    double setPoint_x, setPoint_y, setPoint_vx, setPoint_vy;
    double tau=runTime*setpoint->ballSpeed/setpoint->pathRadius;   
//    cvZero( setpoint->integrationErr );
    
    setPoint_x=setpoint->pathRadius*cos(tau)+cvmGet( setpoint->tableCenter, 0, 0 );
    setPoint_y=0.5*setpoint->pathRadius*sin(tau)+cvmGet( setpoint->tableCenter, 0, 1 );
    setPoint_vx=-setpoint->ballSpeed*sin(tau);
    setPoint_vy=setpoint->ballSpeed*cos(tau);
    
    cvmSet( setpoint->setPoint, 0, 0, setPoint_x );
    cvmSet( setpoint->setPoint, 1, 0, setPoint_vx );
    cvmSet( setpoint->setPoint, 2, 0, setPoint_y );
    cvmSet( setpoint->setPoint, 3, 0, setPoint_vy );
    
    return;
}

void spiralChase( SetPoint *setpoint, double runTime ) {
    double setPoint_x, setPoint_y, setPoint_vx, setPoint_vy;
    double tau=runTime*setpoint->ballSpeed/setpoint->pathRadius;   
    double spiralRadius=(0.5+0.5*sin(tau/5))*setpoint->pathRadius;
//    cvZero( setpoint->integrationErr );

    
    setPoint_x=spiralRadius*cos(tau)+cvmGet( setpoint->tableCenter, 0, 0 );
    setPoint_y=spiralRadius*sin(tau)+cvmGet( setpoint->tableCenter, 0, 1 );
    setPoint_vx=-spiralRadius*sin(tau);
    setPoint_vy=spiralRadius*cos(tau);
    
    cvmSet( setpoint->setPoint, 0, 0, setPoint_x );
    cvmSet( setpoint->setPoint, 1, 0, setPoint_vx );
    cvmSet( setpoint->setPoint, 2, 0, setPoint_y );
    cvmSet( setpoint->setPoint, 3, 0, setPoint_vy );
    
    return;
}

void figure_eightChase( SetPoint *setpoint, double runTime ) {
    double setPoint_x, setPoint_y, setPoint_vx, setPoint_vy;
    double tau=runTime*setpoint->ballSpeed/setpoint->pathRadius;   
//    cvZero( setpoint->integrationErr );
    
    setPoint_x=setpoint->pathRadius*sin(tau)+cvmGet( setpoint->tableCenter, 0, 0 );
    setPoint_vx=setpoint->pathRadius*cos(tau);
    
    cvmSet( setpoint->setPoint, 0, 0, setpoint->pathRadius*sin(tau)+cvmGet( setpoint->tableCenter, 0, 0 ) );
    if (sin(tau/2) > 0){
        setPoint_y=-setpoint->pathRadius*cos(tau)+cvmGet( setpoint->tableCenter, 0, 1 )+setpoint->pathRadius;
        setPoint_vy=setpoint->ballSpeed*sin(tau);
//        cvmSet( *setPoint, 2, 0, -pathRadius*cos(tau)+cvmGet( tableCenter, 0, 1 )+pathRadius);
    }
    else{
        setPoint_y=setpoint->pathRadius*cos(tau)+cvmGet( setpoint->tableCenter, 0, 1 )-setpoint->pathRadius;
        setPoint_vy=-setpoint->ballSpeed*sin(tau);
//        cvmSet( *setPoint, 2, 0, pathRadius*cos(tau)+cvmGet( tableCenter, 0, 1 )-pathRadius);
    }
    
    cvmSet( setpoint->setPoint, 0, 0, setPoint_x );
    cvmSet( setpoint->setPoint, 1, 0, setPoint_vx );
    cvmSet( setpoint->setPoint, 2, 0, setPoint_y );
    cvmSet( setpoint->setPoint, 3, 0, setPoint_vy );
    
    return;
}

